import lejos.nxt.*;
import lejos.nxt.comm.*;
import java.io.*;
import lejos.nxt.TouchSensor;
/**
 * Test of Java streams over USB.
 * Run the PC example, USBSend, to send data.
 * 
 * @author Lawrie Griffiths
 *
 */
public class Control {
    
    public boolean gripState = false; 
    
    private int prevTacho = 0;
    
    public void setSpeed(int speed) {
        Motor.A.setSpeed(speed);
        Motor.B.setSpeed(speed);
    }
    
    public void setGripState(boolean state) {
        //true = closed, false = open
        this.gripState = state;
    }
    
    public boolean getGripState(){
        return this.gripState;
    }
    
    public void doCommand(String command) {
        if(command.equals("forward")) {
            this.moveForward();
        } else if(command.equals("backward")) {
            this.moveBackward();
        } else if(command.equals("left")) {
            this.moveLeft();
        } else if(command.equals("right")) {
            this.moveRight();
        } else if(command.equals("stop")) {
            this.moveStop();
        } else if(command.equals("recycle")) {
            //this.recycle(true);
        } else if(command.equals("opengrip")) {
            this.grip(false);
        } else if(command.equals("closegrip")) {
            this.grip(true);
        } else {
            this.moveStop();
        }
        
    }
    
    public void moveForward() {
        Motor.A.forward();
        Motor.B.forward();
    }

    private void moveBackward() {
        Motor.A.backward();
        Motor.B.backward();
    }
	
    private void moveLeft() {
        Motor.A.forward();
        Motor.B.backward();
    }

    private void moveRight() {
        Motor.A.backward();
        Motor.B.forward();
    }

    private void moveStop() {
        Motor.A.stop();
        Motor.B.stop();
    }
    
    private void grip(boolean closed){
        int degrees;
        boolean proceed = false;
        Motor.C.setSpeed(300);
        if(Motor.C.getTachoCount() != 0 && !closed) {
            degrees = Math.abs(Motor.C.getTachoCount());
            Motor.C.rotate(degrees, true);
        }else if(closed){
        	Motor.C.resetTachoCount();
            degrees = -120;
            Motor.C.rotate(degrees,true);
            //Motor.C.backward(); 
           // while(Motor.C.getLimitAngle())
        }else{
        	
        }
    }
}

